package org.scalpel.locodo;

import org.scalpel.common.KeyPoint;
import org.scalpel.mainApps.IoioMotorControler;

import android.util.Log;


/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * Locomotion : Compute motor speed on a differential drive according
 * to a dynamic target on a 2D plan.
 * It need to be configure with a class instance inherited from MotorInterface.
 * Geometric parameter of wheel are needed to convert encoder value to 
 * meters and radiant.
 * International Metric System is systematically used. 
 **/
public class Locomotion extends Odometry {

  public Locomotion( String name, double encoderWheelPerimeter,
      double encoderWheelsDistanceToCenter, int encoderTickPerRotation) {
    
    super(name, encoderWheelPerimeter, encoderWheelsDistanceToCenter, encoderTickPerRotation);
    
    // Create the odometric engine. It needs encoder wheel perimeter, 
    // distance from the odometric wheel to the center of the robot
    // and the number of tick per rotation of the encoder wheel
    
    // PID parameters must be set within an accessor
    // Do not set the pid parameter here. Use accessor to set them.
    fAnglePid = new PID();
    fAnglePid.P = 1.;
    fAnglePid.I = 0.;
    fAnglePid.D = 0.;
    fAnglePid.iLimit = 10.;
    fDistancePid = new PID();
    fDistancePid.P = 1.;
    fDistancePid.I = 0.;
    fDistancePid.D = 0.;
    fDistancePid.iLimit = 10.;
    
    // initialization
    fMotorLeft = null;
    fMotorRight = null;
    
    fMaxSpeed = 1.5;
    
    fOldDeltaDistance = 0.;
    fOldDeltaAngle = 0.;
    
    // default robot precision
    fDistancePrecision = 0.02; // Default distance precision : 2 cm 
    fAnglePrecision = 5.0* Math.PI / 180.0; // Default angle precision : 5 degrees 
    
    // set current position to (0,0)
    fFieldPosition = new KeyPoint();
    fFieldPosition.x = 0;
    fFieldPosition.y = 0;
    fFieldPosition.a = 0;
    fFieldPosition.d = 0;
    
    // initial target Reached
    fTarget = new KeyPoint(fFieldPosition);
    fTargetReached = true;
    
    
    // default values of acceleration limit

    fAngleAccLimit = 0.06;     // rad/cycle
    fDistanceAccLimit = 0.021; // meter/cycle 
    
    
    // stop signal initialization
    fRegularFreeze = false;
    fEmergencyFreeze = false;
    
    // ramp enable initialization
    fAngleRampEnable = true;
    fDistanceRampEnable = true;
    
    fLastCurrLeftEncoderValue = 0;
    fLastCurrRightEncoderValue = 0;
    
    fReverse = false;
    fMirror = false;
    fAvoidUTurn = true;
    
    fRealSpeed = new double[2];
    fRealSpeed[0] = 0.;
    fRealSpeed[1] = 0.;
    
    fSpeed = new double[2];
    fSpeed[0] = 0.;
    fSpeed[1] = 0.;
    
  }
  
  public void disableAngleControl(boolean disable, double valueWhenDisable){
    fDisableAngleControl = disable;
    fDisableAngleControlStaticSpeed = valueWhenDisable;
  }
  
  public void disableDistanceControl(boolean disable, double valueWhenDisable){
    fDisableDistanceControl = disable;
    fDisableDistanceControlStaticSpeed = valueWhenDisable;
  }
  
  /**
   * set right and left motor instance
   * @param fMotorLeft2 ODOMotorInterface's pointer of the left motor 
   * @param fMotorRight2 ODOMotorInterface's pointer of the right motor 
   */
  public void setMotors( MotorInterface motorLeft, MotorInterface motorRight){
    fMotorLeft = motorLeft;
    fMotorRight = motorRight;
  }
  
  
  /**
   * update. Must frequent to keep high precision
   * @throws ConnectionLostException 
   */
  synchronized public void update(){
    if( fMotorLeft == null || fMotorRight == null){
      // if motors controller are not set
      Log.e("Loco", "Could not compute odometry : motors controler are not set. Call the setMotors method before updating.");
      return;
    }

    //calls motor controller update
    fMotorLeft.update();
    fMotorRight.update();
    
    // get encoder value back 
    // take care that encoder are in opposite side. Conventionally let's inverse right encoder value.
    int currLeftEncoderValue = fMotorLeft.getEncoderValue();
    int currRightEncoderValue = -fMotorRight.getEncoderValue();
    
    // Apply a mirror to the robot reference (left become right)
    if( fMirror ) 
    { 
      //Encoders can be inverted
    	int temp = currLeftEncoderValue; 
    currLeftEncoderValue = currRightEncoderValue; 
    currRightEncoderValue = temp;
    }
     
    // filter incoherent values

    /*if( Math.abs(currLeftEncoderValue - fLastCurrLeftEncoderValue) > 50. ||
        Math.abs(currRightEncoderValue - fLastCurrRightEncoderValue) > 50. )
    {
      Log.e("Loco", "HHLocomotion: get encoder left " + Integer.toString(currLeftEncoderValue)
          + " right " + Integer.toString(currRightEncoderValue));
      Log.e("Loco", "HHLocomotion: last encoder left " + Integer.toString(fLastCurrLeftEncoderValue)
          + " right " + Integer.toString(fLastCurrRightEncoderValue));
      Log.e("Loco", "Encodeur Value rejected");
      
      //return;
    }
    */
    
    fRealSpeed[0] = currLeftEncoderValue - fLastCurrLeftEncoderValue;
    fRealSpeed[1] = currRightEncoderValue - fLastCurrRightEncoderValue;
    fLastCurrLeftEncoderValue = currLeftEncoderValue;
    fLastCurrRightEncoderValue = currRightEncoderValue;
    
    
    // Update the position on the field
    fFieldPosition = updatePosition( currLeftEncoderValue, currRightEncoderValue );
    //Log.i("Loco", "pos:" + fFieldPosition.toString());
       
    
    KeyPoint t = new KeyPoint();
    // look into target to set if the target is an angle target
    if( fTarget.x == 0.0f && fTarget.y == 0.f && fTarget.a != 0.f)
    {
      t.x = 0.f;
      t.y = 0.f;
      t.a = fTarget.a - fFieldPosition.a ; // make the robot turn inplace
      t.a = t.a % (2.0 * Math.PI);
      t.d = 0.f;
    }else{
      // consider target as a field based target
      t = getRelativeTargetPosition( fTarget );
    }
    
    
    // if the target already reached return
    /* keep the robot enslave even if the target is reached
     * if( fTargetReached )
     * return;
     */
    
    // Determine if the target is reach
    if( ( !(t.x == 0. && t.y == 0.) && Math.abs(t.d) < fDistancePrecision) || ( t.x == 0. && t.y == 0. && Math.abs(t.a) < fAnglePrecision) )
      {
      
    	
    	if( !fTargetReached ){
        Log.i("Loco", "target reached");
      
      }
      fTargetReached = true;
      /**Pour retirer "l'asservissement absolue" au millimètre sur angle*/
      fDisableAngleControl =true;
      fMotorLeft.setSpeed( 0.0f );
       fMotorRight.setSpeed( 0.0f );
       //IoioMotorControler.setSpeed(0.0f);
      /*
       * JM : Modif to keep the robot enslave even if the target is reached
      *fMotorLeft.setSpeed( 0. );
       * fMotorRight.setSpeed( 0. );
       * return;
       */
    }else
    {  
      fTargetReached = false;
      fDisableAngleControl = false;
    }
     
    // manage the flip option
    // Conflit between angle and reverse --> symetric angle (90 --> -90) & (0 --> pi)
    /*Without this condition, it reach to replace in front of.*/
    
    if( fReverse || (fAvoidUTurn && ( Math.abs(t.a) > Math.PI / 2. )))
    {
      t.a = t.a + Math.PI;
      t.a = t.a % (2. * Math.PI);
      if( t.a >  Math.PI ) t.a -= 2.0f * Math.PI;
      if( t.a < -Math.PI ) t.a +=  -2.0f * Math.PI;
    }
    
    
    /*
    // send new command to the pid
    // if UTurn avoidance is disable invalidate angle control if the target has been reached
    if( !fAvoidUTurn && fTargetReached ) 
      t.a = 0;
    */
    
    // Log.d( "Loco", "HHLocomotion: target x:" + fTarget.toString());
    // Log.d( "Loco", "HHLocomotion: trel x:" + trel.toString());

    // if the stop command has been called
    // overwrite target distance
    // the robot is still free to turn in place
    if( fRegularFreeze )
    {
      t.d = 0.f;
      t.a = 0.f;
      fRegularFreeze = false;
    }

    // apply the acceleration limitation
    if( fAngleRampEnable ) t.a = xAngleAccLimit( t.a );
    if( fDistanceRampEnable ) t.d = xDistanceAccLimit( t.d );
    
    fAnglePid.setCurrValue( t.a, 1);
    fDistancePid.setCurrValue( t.d, 1);

    // compute pid 
    double newAngleSpeed    = fAnglePid.update();
    double newDistanceSpeed = fDistancePid.update();
    
    // Check if distance or angle controls are enabled
    if( fDisableDistanceControl ) 
      newDistanceSpeed = fDisableDistanceControlStaticSpeed;
    if( fDisableAngleControl ) 
      newAngleSpeed = fDisableAngleControlStaticSpeed;
    
    double ratio = 1. - Math.abs(newAngleSpeed);
    fSpeed[0] = ratio * ratio * newDistanceSpeed - newAngleSpeed; // LEFT
    fSpeed[1] = ratio * ratio * newDistanceSpeed + newAngleSpeed; // RIGHT

    
    /*// Mixes angle command and straight command
    fSpeed[0] = newDistanceSpeed -  newAngleSpeed/2.f; // LEFT
    fSpeed[1] = newDistanceSpeed + newAngleSpeed/2.f; // RIGHT
    // TODO : check this again
    if( fSpeed[0] > fSpeed[1] && fSpeed[0] > 1. ){
      //fSpeed[1] = fSpeed[1] / fSpeed[0];
      fSpeed[1] = fSpeed[1] / fSpeed[0];
      fSpeed[0] = 1.;
    }else{
      if( fSpeed[1] > fSpeed[0] && fSpeed[1] > 1. ){
        fSpeed[0] = fSpeed[0] / fSpeed[1];
        fSpeed[1] = 1.;
      }
    }
*/
    // Apply a mirror to the robot reference (left become right) 
    if( fMirror ) 
    { 
    	// Speeds can be inverted
    	double temp = fSpeed[0];
    	fSpeed[0] = fSpeed[1]; 
        fSpeed[1] = temp;
    } 
     
    // energencyStop method has been called
    if( fEmergencyFreeze )
    {
      Log.i("Loco", "Emergency freeze has been called. ODOLocomotion stop!");
      fMotorLeft.setSpeed( 0.0f );
      fMotorRight.setSpeed( 0.0f );
    }else
    {
      // callback motor controller to send the speed command
      //ODO_DEBUG_LOCO( newDistanceSpeed  << "\t" << newAngleSpeed << "\t" << currLeftEncoderValue << "\t" << currRightEncoderValue << "\t" << t.a << "\t" << t.d );
      //fMotorLeft.setSpeed( fMaxSpeed * fSpeed[0] );
      fMotorLeft.setSpeed( fMaxSpeed * fSpeed[0] );
      fMotorRight.setSpeed( -fMaxSpeed * fSpeed[1] ); // motor are oriented in a opposite way. 
    }
  }
  
  
  /**
   * set current Target.
   * @param target new Target to be reached. <BR>
   * A specific case is done if target.x and target.y are set to 0, 
   * then we consider target as a angle target, in this case X and Y coordinates are ignored.
   * d is systematically ignored
   */
  synchronized public void setTarget( KeyPoint target ){
    Log.i("Loco", "ODOLocomotion::setTarget");
    fTarget = new KeyPoint(target);
    Log.i("Loco", " new target is x " + fTarget.toString()); 
    fTargetReached = false;
  }
  /**
   * @return the current robot target
   */
  public KeyPoint getTarget(){ return fTarget; };
  
  
  synchronized public double[] getRealSpeed(){ return fRealSpeed;};
  synchronized public double[] getSpeed(){ return fSpeed;};
  
  
  /**
   * @return true if target position is under the defined precision
   */
  public boolean isTargetReached(){ return fTargetReached;};
  
  
  /*======================
   * accessors
  ====================== */
  /** @param set the PID coef of the angle pid regulator */
  public void setAnglePID( double p, double i, double d)
  { 
    fAnglePid.P = p; 
    fAnglePid.I = i; 
    fAnglePid.D = d; 
  };
  
  /** @return return current pid coefficient of the angle pid regulator */
  public double[] getAnglePID()       { return new double[]{fAnglePid.P, fAnglePid.I, fAnglePid.D}; };

  /** @param set the PID coef of the distance pid regulator */
  public void setDistancePID( double p, double i, double d)
  { 
    fDistancePid.P = p; 
    fDistancePid.I = i; 
    fDistancePid.D = d; 
  };
  
  /** @return return current pid coefficient of the distance pid regulator */
  public double[] getDistancePID()    { return new double[]{fDistancePid.P, fDistancePid.I, fDistancePid.D}; };

  /** @param pAngle Desire angle precision of the angle regulator */
  public void setAnglePrecision( double pAngle){ fAnglePrecision = pAngle; };
  
  /** @return the desire angle precision of the angle regulator */
  public double getAnglePrecision( ){ return fAnglePrecision; };
  
  /** @param pDistance Desire angle precision of the angle regulator */
  public void setDistancePrecision( double pDistance){ fDistancePrecision = pDistance; };
  
  /** @return the desire distance precision of the angle regulator */
  public double getDistancePrecision( ){ return fDistancePrecision; };
  
  /** @param maxSpeed Maximum motor speed */
  public void setMaxSpeed( double maxSpeed ) { fMaxSpeed = maxSpeed; };
  
  /** @return Current maximum motor speed */
  public double getMaxSpeed( ) { return fMaxSpeed; };

  /** @param accLimit Distance target acceleration limit */
  public void setDistanceAccLimit(double accLimit){ fDistanceAccLimit = accLimit; };
  
  /** @param accLimit Angle target acceleration limit */
  public void setAngleAccLimit(double accLimit){ fAngleAccLimit = accLimit; };
  
  /** @return the current distance target acceleration limit */
  public double getDistanceAccLimit(){ return fDistanceAccLimit;};
  
  /** @return the current angle target acceleration limit */
  public double getAngleAccLimit(){ return fAngleAccLimit;};
  
  
  /**
   * Flip robot back and front if setReverse is called with true as parameter.
   */
  synchronized public void setReverse( boolean onOff) { fReverse = onOff; };
   
  
  /**
   * Return the current reverse status
   */
  public boolean getReverse( ) { return fReverse; };

  
  /**
   * Stop with respect of the limit acceleration criteria.
   * These methods needs to be called before each update !
   */
  public void freeze(){ fRegularFreeze = true; };
    
  
  /**
   * Stop the robot as soon as possible without respect 
   * of the acceleration limit.
   */
  public void emergencyFreeze(){ fEmergencyFreeze = true; };
  
  
  /**
   * Release the robot from emergency freeze state.
   */ 
  public void emergencyUnfreeze(){ fEmergencyFreeze = false;};
  
  
  /**
   * Disable distance regulator acceleration ramp
   */
  public void setDistanceRampEnable( boolean onOff ) { fDistanceRampEnable = onOff; };
  
  
  /**
   * Disable angle regulator acceleration ramp
   */
  public void setAngleRampEnable( boolean onOff ) { fAngleRampEnable = onOff; };
  
  
  /**
   * Apply a mirror to the robot reference (left become right)
   */
  public void setMirror( boolean OnOff ) { fMirror = OnOff; };
  
  
  /**
   * Return true if the mirror mode is on
   */
  public boolean getMirror() { return fMirror; };

  /** Avoid the robot to do a U turn if true */
  public void setUTurnAvoidance( boolean OnOff ){ fAvoidUTurn = OnOff; };

  /** Return the state of U turn avoidance*/
  public boolean getUTurnAvoidance( ){ return fAvoidUTurn; };
  
  /**
   * Apply a limit acceleration on the Angle pid regulator input. This limit is define by the user.
   */
  private double xAngleAccLimit( double in ){
    double tmpAngle = in;
    if( in < fOldDeltaAngle - fAngleAccLimit)
      tmpAngle = fOldDeltaAngle - fAngleAccLimit;
    if( in > fOldDeltaAngle + fAngleAccLimit)
      tmpAngle = fOldDeltaAngle + fAngleAccLimit;
    fOldDeltaAngle = tmpAngle;
    return tmpAngle;
  }
  
  
  /**
   * Apply a limit acceleration on the Distance pid regulator input. This limit is define by the user.
   */
  private double xDistanceAccLimit( double in ){
    double tmpDistance = in;
    if( in < fOldDeltaDistance - fDistanceAccLimit)
      tmpDistance = fOldDeltaDistance - fDistanceAccLimit;
    if( in > fOldDeltaDistance + fDistanceAccLimit)
      tmpDistance = fOldDeltaDistance + fDistanceAccLimit;
    fOldDeltaDistance = tmpDistance;
    return tmpDistance;
  }

  
  /** pid parameters of the angle regulator */
  private PID fAnglePid;
  /** pid parameters of the distance regulator */
  private PID fDistancePid;
  
  
  /** current position in the field space */
  private KeyPoint fFieldPosition;
  
  /** current target in the field space */
  private KeyPoint fTarget;
    
  /** Pointer to the left motor controller */
  private MotorInterface fMotorLeft;
  
  /** Pointer to the right motor controller */
  private MotorInterface fMotorRight;
  
  /** if the target is reached */
  public boolean fTargetReached;
  
  /** precision values (angle, distance) */
  private volatile double fDistancePrecision;
  private volatile double fAnglePrecision;
  
  /** max speed */
  private volatile double fMaxSpeed;
  
  /** backup of the previous Angle and Distance.
   * Use to apply ramp on the pid input data 
   */
  private double fOldDeltaDistance;
  private double fOldDeltaAngle;
  
  /** acceleration limits */
  private volatile double fDistanceAccLimit;
  private volatile double fAngleAccLimit;
  
  /** stop signal */
  private boolean fRegularFreeze;
  private boolean fEmergencyFreeze;
  
  /** disable ramp signal */
  private boolean fDistanceRampEnable;
  private boolean fAngleRampEnable;
  

  private int fLastCurrLeftEncoderValue;
  private int fLastCurrRightEncoderValue;
  
  /** Apply a mirror to the robot reference (left become right) */ 
  private boolean fMirror;
  /** make the robot flip front and back*/
  private boolean fReverse;
  
  /** Speed measure from encoder value */ 
  private double[] fRealSpeed;
  
  /** Speed command */
  private double[] fSpeed;

  /** Allow to disable angle control regulator */
  private boolean fDisableAngleControl = false;

  /** Allow to disable distance control regulator */
  private boolean fDisableDistanceControl = false;

  /** If angle regulator is disable, user could define a default speed command */
  private double fDisableAngleControlStaticSpeed = 0.;

  /** If Distance regulator is disable, user could define a default distance speed command */
  private double fDisableDistanceControlStaticSpeed = 0.;
  
  /** Avoid U turn */
  private boolean fAvoidUTurn;

}
